using System;
using Microsoft.SPOT;
using Netduino4Fun.Framework.ElectronicBrick;
using SecretLabs.NETMF.Hardware.NetduinoPlus;
using System.Threading;
using Netduino4Fun.Framework;

namespace Netduino4Fun.Robot
{
    public class RobotController2
    {
        const int rangeLimit = 300;

        PushButton button = new PushButton(Pins.GPIO_PIN_D8);
        RangeSensor rangeSensor = new RangeSensor(Pins.GPIO_PIN_A1);
        WheelController motorShield = new WheelController();

        bool running = false;
        bool turnLeft = true;

        public void Start()
        {
            button.Push += new Microsoft.SPOT.Hardware.NativeEventHandler(button_Push);

            do
            {
                if (running)
                {
                    int range = rangeSensor.Value;

                    if (range > rangeLimit)
                    {
                        motorShield.Stop();
                        Pause();

                        if (turnLeft)
                            motorShield.Turn90Left();
                        else
                            motorShield.Turn90Right();
                        Pause();

                        motorShield.StepForward();
                        Pause();
                        
                        if (turnLeft)
                            motorShield.Turn90Left();
                        else
                            motorShield.Turn90Right();
                        Pause();

                        motorShield.GoForward(50);
                        
                        //turnLeft = !turnLeft;
                    }
                }


                Thread.Sleep(200);

            } while (true);
        }

        void Pause()
        {
            Thread.Sleep(500);
        }

        void button_Push(uint data1, uint data2, DateTime time)
        {
            running = !running;

            if (running)
                motorShield.GoForward(50);
            else
                motorShield.Stop();
        }
    }
}
